package server.model.solver.maze.algorithms.a_star;

import java.util.ArrayList;
import java.util.PriorityQueue;

import server.model.solver.maze.algorithms.*;

public class AStar {
	private IDomain domain;
	private IDistance h;
	private IDistance g;
	private int pathScore;
	
	public AStar(IDomain domain, IDistance g, IDistance h) {
		this.domain = domain;
		this.h = h;
		this.g = g;
		pathScore = 0;
	}
	
	public int getPathScore() {
		return pathScore;
	}

	public ArrayList<IAction> search(State start, State goal) {
		ArrayList<State> closedList = new ArrayList<State>();
		PriorityQueue<State> openList = new PriorityQueue<State>();
		openList.add(start);
		
		start.setG(0);
		start.setF(g.getDistance(start, start) + h.getDistance(start, goal));

		while (!openList.isEmpty()) {
			State q = openList.poll();
			
			if (q.equals(goal)) {
				pathScore = (int)q.getG();
				return reconstructPath(start, q);
			}

			closedList.add(q);

			for (IAction action : domain.getActions(q)) {
				State qTag = action.doAction(q);
				qTag.setG(g.getDistance(start, qTag));

				double tentative_g_score = q.getG() + domain.distance(q, qTag);

				if (closedList.contains(qTag) && tentative_g_score >= qTag.getG()) {
					continue;
				}

				if (!openList.contains(qTag) || tentative_g_score < qTag.getG()) {
					qTag.setCameFromState(q);
					qTag.setCameFromAction(action);
					
					qTag.setG(tentative_g_score);
					qTag.setF(qTag.getG() + h.getDistance(qTag, goal));
				}
				
				if (!openList.contains(qTag)) {
					openList.add(qTag);
				}
			}
		}
		
		return new ArrayList<IAction>();
	}
	
	private ArrayList<IAction> reconstructPath(State start, State goal) {
		if (goal.getCameFromState() != null) {
			ArrayList<IAction> path = reconstructPath(start ,goal.getCameFromState());
			path.add(goal.getCameFromAction());
			return path;
		}	
		return new ArrayList<IAction>();
	}
}
